#include "SypEngine.h"

#ifdef CHECKMEM
#include "mmgr.h"
#endif

namespace Syp
{
	Skeleton::Skeleton(const String& name):Resource(name),AnimLength(0.0f)
	{
	}


	PInt Skeleton::addAnimSet(AnimSet& as)
	{
		animSets.push_back(as);
		return static_cast<PInt>(animSets.size()-1);
	}

	Skeleton* Skeleton::createSkeleton(const String& name)
	{
		return (new Skeleton(name));
	}
	void Skeleton::destroySkeleton(Skeleton* sk)
	{
		delete sk;
	}

	PInt Skeleton::getNumBones()
	{
		return static_cast<PInt>(Bones.size());
	}

	PInt Skeleton::getBoneID(const String& name)
	{
		std::vector<Bone*>::iterator i = Bones.begin();

		while(i != Bones.end())
		{
			if(name == (*i)->name)
			{
				return (*i)->ID;
			}
			++i;
		}
		//Error no bone found
		return 0;
	}

	Bone& Skeleton::getBoneRef(const String& name)
	{
		std::vector<Bone*>::iterator i = Bones.begin();

		while(i != Bones.end())
		{
			if(name == (*i)->name)
			{
				return (*(*i));
			}
			++i;
		}
		//Error no bone found
		assert(0 && "This bone does not exist and getBoneRef has failed");
		return (*(*i));
	}

	Bone& Skeleton::getBoneRef(const short ID)
	{
		//std::vector<Bone*>::iterator i = Bones.begin();

		//while(i != Bones.end())
		//{
		//	if(ID == (*i)->ID)
		//	{
		//		return (*(*i));
		//	}
		//	++i;
		//}
		////Error no bone found
		//assert(0 && "This boneID does not exist and getBoneRef has failed");
		//return (*(*i));
		return (*Bones[ID]);
	}

	std::vector<Bone*>& Skeleton::getBonesRef()
	{
		return Bones;
	}

	float Skeleton::getAnimLength()
	{
		return AnimLength;
	}

	const AnimSet& Skeleton::getAnimSet(PInt index)
	{
		return animSets[index];
	}

	void Skeleton::writeToFile(const String& name)
	{
		OutputStream bo;
		bo.open(name.c_str(),std::ios::binary);
		if( bo.is_open() )
		{
			PInt temp;
			temp = static_cast<PInt>(Bones.size());
			//Number of Bones
			bo.write( reinterpret_cast<char*>(&temp), sizeof(temp));
			for(PInt i=0;i<temp;++i)
			{
				//ID of bone
				bo.write( reinterpret_cast<char*>(&Bones[i]->ID), sizeof(short));
				//Position of bone
				Point4 pos = Bones[i]->getPosition();
				Quat or = Bones[i]->getOrientation();
				//x
				bo.write( (char*)(&pos.x), sizeof(float));
				bo.write( (char*)(&pos.y), sizeof(float));
				bo.write( (char*)(&pos.z), sizeof(float));
				//Rotation
				bo.write( (char*)(&or.x), sizeof(float));
				bo.write( (char*)(&or.y), sizeof(float));
				bo.write( (char*)(&or.z), sizeof(float));
				bo.write( (char*)(&or.w), sizeof(float));
				//Collada's bone inv matrix
				bo.write( (char*)(&Bones[i]->inv2), sizeof(float) * 16 );



				//Parent Information
				Bone* parentPtr = (Bone*)(Bones[i]->getParentPtr());		
				if(parentPtr)
				{
					bo.write( (char*)(&parentPtr->ID), sizeof(short));
				}
				else
				{
					short heehee = -1;
					bo.write( (char*)(&heehee), sizeof(short));

				}

				//Keyframe data
				PInt temp2;
				temp2 = static_cast<PInt>(Bones[i]->KFrame.size());
				//Num of frames
				bo.write( reinterpret_cast<char*>(&temp2), sizeof(temp2));
				for(PInt j=0;j<temp2;++j)
				{
					bo.write( reinterpret_cast<char*>(&Bones[i]->KFrame[j].time), sizeof(float));
					//Position of bone
					//x
					bo.write( (char*)(&Bones[i]->KFrame[j].Pos.x), sizeof(float));
					bo.write( (char*)(&Bones[i]->KFrame[j].Pos.y), sizeof(float));
					bo.write( (char*)(&Bones[i]->KFrame[j].Pos.z), sizeof(float));
					//Rotation
					bo.write( (char*)(&Bones[i]->KFrame[j].Rot.x), sizeof(float));
					bo.write( (char*)(&Bones[i]->KFrame[j].Rot.y), sizeof(float));
					bo.write( (char*)(&Bones[i]->KFrame[j].Rot.z), sizeof(float));
					bo.write( (char*)(&Bones[i]->KFrame[j].Rot.w), sizeof(float));				
				}



			}

		}
		bo.close();

	}

	void Skeleton::loadFromFile(const String& name)
	{
		InputStream bi;
		bi.open(name.c_str(),std::ios::binary);
		if( bi.is_open() )
		{
			std::map<short,short> parentMap;
			PInt temp;			
			//Number of Bones
			bi.read( reinterpret_cast<char*>(&temp), sizeof(temp));
			for(PInt i=0;i<temp;++i)
			{
				Bone* bone = new Bone();
				Bone& b = *bone;
				//ID of bine
				bi.read( reinterpret_cast<char*>(&b.ID), sizeof(short));
				//Position of bone 
				float x,y,z,w;
				//x
				bi.read( (char*)(&x), sizeof(float));
				bi.read( (char*)(&y), sizeof(float));
				bi.read( (char*)(&z), sizeof(float));
				b.setPosition(x,y,z);
				//Rotation
				bi.read( (char*)(&x), sizeof(float));
				bi.read( (char*)(&y), sizeof(float));
				bi.read( (char*)(&z), sizeof(float));
				bi.read( (char*)(&w), sizeof(float));
				b.setOrientation(x,y,z,w);

				//Collada's bone inv matrix
				bi.read( (char*)(b.inv2), sizeof(float) * 16 );

				//Parent Information				
				short parentID = -1;
				bi.read( (char*)(&parentID), sizeof(short));				
				parentMap[b.ID] = parentID;

				//Keyframe data
				PInt temp2;				
				//Num of frames
				bi.read( reinterpret_cast<char*>(&temp2), sizeof(temp2));
				for(PInt j=0;j<temp2;++j)
				{
					KeyFrame kf;
					bi.read( reinterpret_cast<char*>(&kf.time), sizeof(float));
					//Position of bone
					//x
					bi.read( (char*)(&kf.Pos.x), sizeof(float));
					bi.read( (char*)(&kf.Pos.y), sizeof(float));
					bi.read( (char*)(&kf.Pos.z), sizeof(float));
					//Rotation
					bi.read( (char*)(&kf.Rot.x), sizeof(float));
					bi.read( (char*)(&kf.Rot.y), sizeof(float));
					bi.read( (char*)(&kf.Rot.z), sizeof(float));
					bi.read( (char*)(&kf.Rot.w), sizeof(float));

					b.KFrame.push_back(kf);
				}

				if(b.KFrame[temp2-1].time > AnimLength)
				{
					AnimLength = b.KFrame[temp2-1].time;
				}

				Bones.push_back(bone);
				std::cout<<"Last entry into Bones array has a parent : "<<Bones[ Bones.size()-1]->getParentPtr()<<"\n";

			}
			for(PInt i=0;i<temp;++i)
			{
				if(parentMap[Bones[i]->ID] != -1)
				{
					Bones[i]->bindToNode( &getBoneRef( parentMap[Bones[i]->ID] ) );
				}
			}

		}
		bi.close();
		std::vector<Bone*>::iterator BBegin = Bones.begin();
		std::vector<Bone*>::iterator BEnd = Bones.end();	

		while(BBegin != BEnd)
		{
			calcBoneInverse(*(*BBegin));
			++BBegin;
		}

	}

	Skeleton::~Skeleton()
	{
		std::vector<Bone*>::iterator BBegin = Bones.begin();
		std::vector<Bone*>::iterator BEnd = Bones.end();

		while(BBegin != BEnd)
		{
			delete *BBegin;
			++BBegin;
		}



	}

	void Skeleton::calcBoneInverse(Bone& b)
	{
		Bone* parent = reinterpret_cast<Bone*>(b.getParentPtr());
		//dont bother if it is already calculated
		if(b.invCalculated)
		{
			return;
		}
		//Calculate Bones Inverse
		float InvPos[16];
		float InvRot[16];
		float tTrans[16];

		//Position
		Math::IdentityMatrix(InvPos);
		InvPos[12] = -b.getPosition().x;
		InvPos[13] = -b.getPosition().y;
		InvPos[14] = -b.getPosition().z;
		//Rotation
		Quat inv = b.getOrientation();
		inv.x = -inv.x;
		inv.y = -inv.y;
		inv.z = -inv.z;		
		Math::QuatToMatrix(inv,InvRot);
		if( !parent )
		{
			//Translate to origin, then apply inverse rotation	
			Math::multMat(InvRot,InvPos,b.inv);	
		}
		else
		{
			Math::multMat(InvRot,InvPos,tTrans);
			//Before we proceed, parent must be calculated
			if(!parent->invCalculated)
			{
				calcBoneInverse( *parent );
			}

			//Apply after parents inverse
			Math::multMat(tTrans, parent->inv, b.inv);
		}

		Math::IdentityMatrix(b.invB);


		b.invB[12] = b.getPosition().x;
		b.invB[13] = b.getPosition().y;
		b.invB[14] = b.getPosition().z;

		////Hack
		b.invCalculated = true;

	}

}
